/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Timer;
//import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team3502Robot extends SimpleRobot {
        RobotDrive drive = new RobotDrive(4, 2, 3, 1); //RobotDrive(int frontLeftMotor, int rearLeftMotor, int frontRightMotor, int rearRightMotor, double sensitivity)
        Joystick leftStick = new Joystick(1); //joystick on port 1 drives the robot
        Joystick rightStick = new Joystick(2); //joystick on port 2 operates the arm
        Victor shoulder_motor = new Victor(4,5);  //slot 4, channel 5
        Victor elbow_motor = new Victor(4,6);
        Compressor kompressor = new Compressor(8, 1); //Compressor(int pressureSwitchChannel, int compressorRelayChannel)
        DoubleSolenoid grip = new DoubleSolenoid(3, 4);
        DoubleSolenoid mbdeploy = new DoubleSolenoid(1, 2);
        LineSensor tracklines = new LineSensor(1, 3, 5, drive);
        hiphop dougie = new hiphop(0.5, 0.5, drive, leftStick); //(initial maxspeed, initial steptime, drive, joystick)


/*
    public void camerainit()  {
        Timer.delay(8);
        AxisCamera camera = AxisCamera.getInstance();
    }
*/


     /*
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        while (isAutonomous() && isEnabled()){
            tracklines.run();
            /* autonomous code for hanging tubes
            shoulder_motor.set(0); //make sure window motors are initially stationary
            elbow_motor.set(0);
            shoulder_motor.set(0.5);
            elbow_motor.set(0.5);  //run window motors at 50% speed in one direction
            shoulder_motor.set(-0.5);
            elbow_motor.set(-0.5);  //run window motors at 50% speed in the other direction
            shoulder_motor.set(0);
            elbow_motor.set(0);  //stop window motors
            */
        }
    }
    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {

        //initialize variables
        int armspeed, drivespeed, full, half, slow;
        boolean laststate8, laststate10, laststate12, laststateTrigger;
        full = 1;
        half = 2; //half speed
        slow = 3; //33.33% speed
        drivespeed = slow; //default speed is set to 33% for safety
        armspeed = half; //default speed for arm is set to 50%
        laststate8 = laststate10 = laststate12 = laststateTrigger = false;
        grip.set(DoubleSolenoid.Value.kReverse);
        mbdeploy.set(DoubleSolenoid.Value.kReverse);

        //variables for dougie dance
        boolean laststate11, laststate9;
        laststate11 = laststate9 = false;

        while (isOperatorControl() && isEnabled()){ // loop until change

            //hip hop dance sequence

             //start the dance sequence using button 11
                if (leftStick.getRawButton(11) == true){
                    while (leftStick.getRawButton(9) == false){ //hold button 9 to stop dance
                        //sequences: 1 = strafe LRLL, 2 = strafe RLRR, 3 = Twist LRLL, 4 = twist RLRR
                        dougie.dance(1);
                        dougie.dance(2);
                        dougie.dance(1);
                        dougie.dance(2);
                        dougie.dance(3);
                        dougie.dance(4);
                    }
                }


            //turns on compressor, turns off automatically when pressure reads 'full'
            //kompressor.start();


            //uses left joystick Trigger button to switch solenoid that controls the minibot deployer,
            if (leftStick.getTrigger() == true && laststateTrigger == false) {
                if (mbdeploy.get() == DoubleSolenoid.Value.kReverse) {
                    mbdeploy.set(DoubleSolenoid.Value.kForward);
                } else {
                    mbdeploy.set(DoubleSolenoid.Value.kReverse);
                }
            }
            laststateTrigger = leftStick.getTrigger();


            //uses left joystick button 10 button to switch solenoid that controls the tube trapper
            //should change to rightStick Trigger button when ready
            if (leftStick.getRawButton(10) == true && laststate10 == false) {
                if (grip.get() == DoubleSolenoid.Value.kReverse) {
                    grip.set(DoubleSolenoid.Value.kForward);
                } else {
                    grip.set(DoubleSolenoid.Value.kReverse);
                }
            }
            laststate10 = leftStick.getRawButton(10);


            //uses buttons 12 and 7 simultaneously to switch to full (100%) driving speed
            //button 12 by itself switches to slow (20%) driving speed
            if (leftStick.getRawButton(12) == true && laststate12 == false) {
                if (drivespeed == slow && leftStick.getRawButton(7) == true) {
                    drivespeed = full;
                }
                else {
                    drivespeed = slow;
                }
            }
            laststate12 = leftStick.getRawButton(12);


            //uses buttons 8 and 9 simultaneously to switch to full (100%) arm speed
            //button 8 by itself switches to half (50%) driving speed
            if (rightStick.getRawButton(8) == true && laststate8 == false) {
                if (armspeed == half && rightStick.getRawButton(9) == true) {
                    armspeed = full;
                }
                else {
                    armspeed = half;
                }
            }
            laststate8 = rightStick.getRawButton(8);


            //commands to drive mecanum wheels
            double x, y, z, throt;
            //when throttle is at 0 (all the way down), speed is reduced by 25%; when throttle is at 1 (all the way up), speed is 100%
            throt = ((leftStick.getThrottle()+1)*3/2)+1; //change down to up throttle values fom 4 to 1 instead of 1 to -1
            x = leftStick.getX()/throt;
            y = leftStick.getY()/throt;
            z = leftStick.getTwist()/throt;
            drive.mecanumDrive_Cartesian(x/drivespeed, y/drivespeed, z/drivespeed, 0); //drive with joystick


            //commands to operate window motors on arm
            shoulder_motor.set(rightStick.getY()/armspeed); //drive shoulder joint with joystick up and down
            elbow_motor.set(rightStick.getX()/armspeed); //drive elbow joint with joystick left and right


            Timer.delay(0.005);
        }
    }
}
